package stateMachine;
import java.util.ArrayList;

import gui.OpCom;
import monitor.ControllerStatMonitor;
import monitor.ModeMonitor;
import monitor.ExcecutionMonitor;
import monitor.StateVariableMonitor;
import monitor.StepMonitor;
import regulators.BallAndBeamRegul;
import regulators.BeamRegul;
import se.lth.control.realtime.AnalogIn;
import se.lth.control.realtime.AnalogOut;
import se.lth.control.realtime.DigitalIn;
import se.lth.control.realtime.DigitalOut;
import states.InitialState;
import states.StateClass;

public class StateMachine extends Thread {

	
	public static BeamRegul 	beam;
	public static BallAndBeamRegul ballAndBeam;
	/**
	 * !boolean = push
	 */
	public DigitalOut digitalSolenoid;

	/**
	 * Analog Input 1: angle  -10 \ (right down) ; +10 / (left down)
	 */
	private AnalogIn analogInAngle;

	/**
	 * Analog Input 2: ball position  - 10 left; +10 right
	 */
	private AnalogIn analogInPosition;

	/**
	 *  Analog Output 0: Motor
	 */
	private AnalogOut analogOut;
	
	private DigitalIn initialPosSensor;

	
	/**
	 * State handling
	 */

	public static StateClass state;
	private boolean initialExcecution;
	private static final boolean STOP 	= false;
	private static final boolean GO 	= true;
	private boolean runSM = true;
	StepMonitor			stepMonitor;
	ModeMonitor			modeMonitor;
	ExcecutionMonitor 	executionMonitor;
	StateVariableMonitor stateVariableMonitor;
	private OpCom op;

	public enum Mode{AUTO, STEP};

	public enum Ball{SMALL, MEDIUM, LARGE, UNKNOWNBALL};



	public StateMachine(OpCom op, ControllerStatMonitor controllerStatMonitor) {
		try { 
			analogInPosition = new AnalogIn(1);
			analogInAngle = new AnalogIn(0);
			digitalSolenoid = new DigitalOut(0);
			analogOut = new AnalogOut(0);
			initialPosSensor = new DigitalIn(0);


		} catch (Exception e) {
			e.printStackTrace();
		}	

		this.op = op;
		stateVariableMonitor = new StateVariableMonitor();
		ballAndBeam		= new BallAndBeamRegul(analogInPosition, analogInAngle,analogOut, 0.0, Thread.MAX_PRIORITY-4,op,stateVariableMonitor, controllerStatMonitor);
		beam 	= new BeamRegul(analogInPosition, analogInAngle, analogOut, 0.0, Thread.MAX_PRIORITY-4,op, stateVariableMonitor, controllerStatMonitor);
		op.setRegul(beam , ballAndBeam);

		state = new StateClass( initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor);
		
		modeMonitor = new ModeMonitor();
		executionMonitor = new ExcecutionMonitor();
		stepMonitor	= new StepMonitor();
		modeMonitor.setMode(Mode.AUTO);
		executionMonitor.setRunning(GO);
		stepMonitor.setNextStep(GO);
		setPriority(Thread.MAX_PRIORITY-2);
		setName("StateMachine");

	}


//
//	public void holdStateMachine() {
//		state.suspend();
//		executionMonitor.setRunning(STOP);
//	}
//
//
//	public void resetStateMachine() {
//		beam.shutDown();
//		ballAndBeam.shutDown();
//		StateClass.setCurrentState(new InitialState(  initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, null));
//	}
//
//
//
//	public void restartStateMachine() {
//		state.resume();
//		executionMonitor.setRunning(GO);
//	}
//
//
//	public void nextStep() {
//		stepMonitor.setNextStep(GO);
//	}
//
//
//	public void setAutoMode() {
//		Mode mode = Mode.AUTO;
//		modeMonitor.setMode(mode);
//		stepMonitor.setNextStep(GO);
//	}
//
//
//	public void setStepMode() {
//		Mode mode = Mode.STEP;
//		modeMonitor.setMode(mode);
//		stepMonitor.setNextStep(STOP);
//	}
//
//
//	public void shutDown() {
//		state.suspend();
//		ballAndBeam.shutDown();
//		beam.shutDown();
//		executionMonitor.setRunning(STOP);
//		runSM = false;
//	}
//
//
//
//

	@Override
	public void run() {
		
		long t ;
		long duration;
		
		state.setCurrentState(new InitialState( initialPosSensor, digitalSolenoid, beam, ballAndBeam, op, stateVariableMonitor));
		initialExcecution = true;
		boolean beamHasrun = false;
		boolean ballAndBeamHasRun = true;
		
		while(runSM) {
			
			if( beam.isRunning() && !beamHasrun){
				op.initializeGUI(1);
				beamHasrun = true;
				ballAndBeamHasRun = false;
			}
			
			
			if( ballAndBeam.isRunning() && !ballAndBeamHasRun){
				op.initializeGUI(2);
				beamHasrun = false;
				ballAndBeamHasRun = true;
			}
			t = System.currentTimeMillis();
			if (executionMonitor.isRunning()) {
				
				if (initialExcecution) {
					state.initialExecution();
					initialExcecution = false;
				} else {
					state.execute();
				}

				if (state.stateOFExcecution()/* && stepMonitor.isNextStep()*/) { // implement when using step
					initialExcecution = true;
					state.nextState();
					if (modeMonitor.getMode() == Mode.STEP) {
						stepMonitor.setNextStep(STOP);
					}
				}
			}	

			t = t+ 20;
			duration = t-System.currentTimeMillis();
			if ( duration > 0) {
				try {
					sleep(duration);
				} catch(InterruptedException e) {
					e.printStackTrace();
				}
			}

		}
	}
	
	
	public void resetStart() {
		stepMonitor.setNextStep(GO);
		
	}

	public void setNextStep() {
		state.nextState();
		
	}



	public ArrayList<double[]> getTracking() {
		return ballAndBeam.getTracking();
		
	}
	
	public ArrayList<Double> getMean() {
		return ballAndBeam.getMean();
		
	}
	
	
}
